#include "ppgrealprocess1.h"
#include "Config/constant.h"
#include "IIRDesign/IIRFilterDesign.h"
#include "QDebug"
PPGRealProcess1::PPGRealProcess1(QObject *parent)
    : QObject{parent}
{
    IIRDesign(0,2,Constant::SampleRate,1.5,5,a_ppg_lp_com,b_ppg_lp_com);//计算低通1.5hz
    IIRDesign(0,2,Constant::SampleRate,5,5,a_ppg_lp,b_ppg_lp);//显示用低通5hz
    IIRDesign(1,2,Constant::SampleRate,0.2,5,a_ppg_hp,b_ppg_hp);
    initial();
}
PPGRealProcess1::~PPGRealProcess1()
{

}

void PPGRealProcess1::initial()
{
    SampleNum=0;SampNumForPI=0;
    for(int i=0;i<101;i++)
    {
        peakWindowIR[i]=0;
        peakWindowRED[i]=0;
    }
    lastPeakIndex=0;
    lastValleyIndex=0;
    smallest=Constant::SampleRate * 60 / 220;
    movingWindowSize =Constant::SampleRate / 50;
    for(int i=0;i<movingWindowSize;i++)
    {
        movingWindowHP.append(0);
        movingWindowRed.append(0);
    }
    peakPos.clear();
    for(int i=0;i<3;i++)
    {
        pi[i]=0;
    }
    for(int i=0;i<12;i++)
    {
        HR[i]=0;
        SpO2[i]=0;
    }
    foundPeak=0;
    totalFoundPeak=0;

    for(int i=0;i<5*Constant::SampleRate;i++)
    {
        PPGForEvaluation.append(0);
    }
    HRVBarLoop=0;

}


void PPGRealProcess1::StartCompute(double ir,double red)
{
    double irdata=lp_filter_ir_forCompute(-1*ir);
    double reddata=lp_filter_red(-1*red);

    //加高通处理，高通后的信号去除直流量，特征点提取更稳定
    double irdata2 = hp_filter_ir(irdata);
    double redata2 = hp_filter_red(reddata);
    movingWindowHP.removeFirst();
    movingWindowHP.append(irdata2);
    movingWindowRed.removeFirst();
    movingWindowRed.append(redata2);
    double m_ir=0;
    double m_red=0;
    for(int k=0;k<movingWindowSize;k++)
    {
        m_ir+=movingWindowHP.at(k);
        m_red+=movingWindowRed.at(k);
    }
    m_ir=m_ir/movingWindowSize;
    m_red=m_red/movingWindowSize;
    SampleNum++; SampNumForPI++;
    updateWindow(peakWindowIR, Constant::SampleRate / 5, m_ir);
    updateWindow(peakWindowRED, Constant::SampleRate / 5, m_red);
    ispeak = 0;
    if (lastPeakIndex > smallest)
    {

        // looking for a local maximum using the 50 point buffer
        ispeak = 1;
        for (int j = Constant::SampleRate / 10; j >= 1; j--)
        {
            if (peakWindowIR[Constant::SampleRate / 10] < peakWindowIR[Constant::SampleRate / 10 - j])
                ispeak = 0;
            if (peakWindowIR[Constant::SampleRate / 10] < peakWindowIR[Constant::SampleRate / 10 + j])
                ispeak = 0;

        }
        if (ispeak == 1)
        {
            lastPeakValueIR = findMax(peakWindowIR,Constant::SampleRate / 5 + 1);
            lastPeakValueRED = findMax(peakWindowRED,Constant::SampleRate / 5 + 1);
            // if we have a local maximum
            int validPeakPos = SampleNum - (Constant::SampleRate / 10 + 1);//实际波峰位置，波形是反的，代表的实际的波谷
            peakPos.push_back(validPeakPos);
            if(lastPeakIndex*2<1500)
                emit sendRR(lastPeakIndex*2,1);

            totalFoundPeak++;
            if (totalFoundPeak > 2)
            {
                // Update the HR buffer
                updateHeartRate(HR, lastPeakIndex);
            }
            lastPeakIndex = 0;
            foundPeak++;
        }

        if ((lastValleyIndex > smallest) && (ispeak == 0))
        {
            // looking for a local minimum using the 20 point buffer
            ispeak = 1;
            for (int j = Constant::SampleRate / 10; j >= 1; j--)
            {
                if (peakWindowIR[Constant::SampleRate / 10] >= peakWindowIR[Constant::SampleRate / 10 - j])
                    ispeak = 0;
                if (peakWindowIR[Constant::SampleRate / 10] >= peakWindowIR[Constant::SampleRate / 10 + j])
                    ispeak = 0;
            }

            // if we have a local minimum
            if (ispeak == 1)
            {
                // values for SPO2 ratio
                lastOnsetValueIR = findMin(peakWindowIR,Constant::SampleRate / 5 + 1);
                lastOnsetValueRED = findMin(peakWindowRED,Constant::SampleRate / 5 + 1);
                totalFoundPeak++;
                if (totalFoundPeak > 2)
                    updateSPO2(SpO2, lastOnsetValueIR, lastOnsetValueRED, lastPeakValueIR, lastPeakValueRED);
                lastValleyIndex = 0;
                foundPeak++;
            }
        }
        if (foundPeak > 2)
        {
            // Every 4 new peaks update return values
            foundPeak = 0;
            unsigned char temp_hr = chooseRate(HR);
            if ((temp_hr > 40) && (temp_hr < 220))
                emit sendHR(temp_hr,1);
            double temp_spo2 = chooseSPox(SpO2);
            if ((temp_spo2 > 70) && (temp_spo2 <= 100)) {
                emit sendSPOX(temp_spo2,1);
            }
        }

    }
    lastPeakIndex++;
    lastValleyIndex++;
    dcIR += irdata;
    dcRed += reddata;

}
/**
     * 功能：红光低通滤波
     * @brief lp_filter_red
     * @param raw_red
     * @return
     */
double PPGRealProcess1::lp_filter_red(double raw_red)
{
    double static i1=0;double static i2=0;double static i3=0;
    double static o1=0;double static o2=0;double static o3=0;
    i1 = i2;i2 = i3;i3 = raw_red;
    o1 = b_ppg_lp[0] * i3 + b_ppg_lp[1] * i2 + b_ppg_lp[2] * i1 - a_ppg_lp[1] * o2 - a_ppg_lp[2] * o3;
    o3 = o2;o2 = o1;
    return o1;
}
/**
     * 功能：红外低通滤波
     * @brief lp_filter_ir
     * @param rawdata
     * @return
     */
double PPGRealProcess1::lp_filter_ir(double raw_ir)
{
    double static i1=0;double static i2=0;double static i3=0;
    double static o1=0;double static o2=0;double static o3=0;
    i1 = i2;i2 = i3;i3 = raw_ir;
    o1 = b_ppg_lp[0] * i3 + b_ppg_lp[1] * i2 + b_ppg_lp[2] * i1 - a_ppg_lp[1] * o2 - a_ppg_lp[2] * o3;
    o3 = o2;o2 = o1;
    return o1;
}

/**
     * 功能：红外低通滤波
     * @brief lp_filter_ir
     * @param rawdata
     * @return
     */
double PPGRealProcess1::lp_filter_ir_forCompute(double raw_ir)
{
    double static i1=0;double static i2=0;double static i3=0;
    double static o1=0;double static o2=0;double static o3=0;
    i1 = i2;i2 = i3;i3 = raw_ir;
    o1 = b_ppg_lp_com[0] * i3 + b_ppg_lp_com[1] * i2 + b_ppg_lp_com[2] * i1 - a_ppg_lp_com[1] * o2 - a_ppg_lp_com[2] * o3;
    o3 = o2;o2 = o1;
    return o1;
}
/**
     * 功能：红光高通滤波
     * @brief hp_filter_red
     * @param raw_red
     * @return
     */
double PPGRealProcess1::hp_filter_red(double raw_red)
{
    double static i1=0;double static i2=0;double static i3=0;
    double static o1=0;double static o2=0;double static o3=0;
    i1 = i2;i2 = i3;i3 = raw_red;
    o1 = b_ppg_hp[0] * i3 + b_ppg_hp[1] * i2 + b_ppg_hp[2] * i1 - a_ppg_hp[1] * o2 - a_ppg_hp[2] * o3;
    o3 = o2;o2 = o1;
    return o1;
}
/**
     * 功能：红外高通滤波
     * @brief hp_filter_ir
     * @param rawdata
     * @return
     */
double PPGRealProcess1::hp_filter_ir(double raw_ir)
{
    double static i1=0;double static i2=0;double static i3=0;
    double static o1=0;double static o2=0;double static o3=0;
    i1 = i2;i2 = i3;i3 = raw_ir;
    o1 = b_ppg_hp[0] * i3 + b_ppg_hp[1] * i2 + b_ppg_hp[2] * i1 - a_ppg_hp[1] * o2 - a_ppg_hp[2] * o3;
    o3 = o2;o2 = o1;
    return o1;
}


void PPGRealProcess1::updateWindow(double* peakWindow, int len,double Y)
{
    unsigned char i;
    for (i = len; i >= 1; i--)
    {
        peakWindow[i] = peakWindow[(unsigned char)(i - 1)];
    }
    peakWindow[0] = Y;
}
void PPGRealProcess1::updateHeartRate(unsigned char* rate, int last)
{
    // Adds a new Heart rate into the array and lose the oldest
    unsigned char i;
    i = 60 * Constant::SampleRate / last;
    if ((i > 40) && (i < 220))
    {
        for (i = 11; i >= 1; i--)
        {
            rate[i] = rate[(unsigned char)(i - 1)];
        }
        rate[0] = 60 * Constant::SampleRate / last;
    }
}

void PPGRealProcess1::updateSPO2(double* SPO2, double lastOnsetValueIR, double lastOnsetValueRed, double lastPeakValueIR, double lastPeakValueRed)
{
    float AC_IR, AC_RED;
    float ac_div, dc_div;
    float xv,ox;
    AC_IR = lastPeakValueIR - lastOnsetValueIR;
    AC_RED = lastPeakValueRed - lastOnsetValueRed;
    if (AC_RED == 0)
        return;
    if (dcIR == 0)
        return;
    ac_div = AC_IR;
    ac_div = ac_div / AC_RED;

    dc_div = dcRed;
    dc_div = dc_div / dcIR;

    xv = ac_div * dc_div;
    if (xv > 2.5)
        xv = 2.5;
    if (xv > 1.65)
        ox = xv + 97.45;
    else
        ox = -2.1891 * xv * xv + 10.031 * xv + 88.277;

    //ox = ox - 5;
    if (ox > 100)
        ox = 99.8;
    if ((ox > 70) && (ox < 100))
    {
        for (int i = 11; i >= 1; i--)
        {
            SPO2[i] = SPO2[(unsigned char)(i - 1)];
        }
        SPO2[0] = ox;
    }
    updatepi(lastOnsetValueIR, lastPeakValueIR);
    dcIR = 0;
    dcRed = 0;
    SampNumForPI = 0;
}

void PPGRealProcess1::updatepi(double lastOnsetValueIR, double lastPeakValueIR)
{
    double temppi = (lastPeakValueIR - lastOnsetValueIR) * 1.70 * SampNumForPI / dcIR;
    temppi = temppi < 0 ? -1 * temppi : temppi;
    temppi = temppi > 8 ? 8 : temppi;
    if (temppi > 0)
    {
        for (int i = 2; i >= 1; i--)
        {
            pi[i] = pi[(unsigned char)(i - 1)];
        }
        pi[0] = temppi;
        double sum = 0;
        int nb = 0;
        for (int i = 0; i < 3; i++)
        {
            sum += pi[i];
            nb += 1;
        }
        if (nb > 0) {
            perfusion = sum / nb;
        }
    }

    perfusion = perfusion * 100;
    emit sendPerfustion(perfusion,1);

}
unsigned char PPGRealProcess1::mean_BP(double* BP)
{
    double sum = 0;
    for (int i = 0; i < 3; i++)
        sum += BP[i];
    return  sum/3;
}
unsigned char PPGRealProcess1::chooseRate(unsigned char* rate)
{
    // Returns the average rate, after removing the lowest and highest values (based on the number of found HR removing 2-4-6 values).
    unsigned char max, min, i, nb;
    unsigned int sum, fullsum;
    max = rate[0];
    min = rate[0];
    sum = 0;
    nb = 0;
    fullsum = 0;
    for (i = 7; i >= 1; i--)
    {
        if (rate[(unsigned int)(i - 1)] > 0)
        {
            if (rate[(unsigned int)(i - 1)] > max)
            {
                max = rate[(unsigned int)(i - 1)];
            }
            if (rate[(unsigned int)(i - 1)] < min)
            {
                min = rate[(unsigned int)(i - 1)];
            }
            sum += rate[(unsigned int)(i - 1)];
            nb++;
        }
    }

    if (nb > 2)
        fullsum = (sum - max - min) * 10 / (nb - 2);
    else if (nb > 0)
        fullsum = (sum) * 10 / (nb);

    sum = fullsum / 10;

    if (fullsum - sum * 10 > 4)
        sum++;
    return sum;
}
double PPGRealProcess1::chooseSPox(double* ox)
{
    // Returns the average ox, after removing the lowest and highest values (based on the number of found ox removing 2-4-6 values).
    float max, min;
    unsigned char i, nb;
    float sum, fullsum;
    max = ox[0];
    min = ox[0];
    sum = 0;
    nb = 0;
    fullsum = 0;
    for (i = 7; i >= 1; i--)
    {
        if (ox[(unsigned int)(i - 1)] > 0)
        {
            if (ox[(unsigned int)(i - 1)] > max)
            {
                max = ox[(unsigned int)(i - 1)];
            }
            if (ox[(unsigned int)(i - 1)] < min)
            {
                min = ox[(unsigned int)(i - 1)];
            }
            sum += ox[(unsigned int)(i - 1)];
            nb++;
        }
    }

    if (nb > 2)
        fullsum = (sum - max - min) * 10 / (nb - 2);
    else if (nb > 0)
        fullsum = (sum) * 10 / (nb);

    sum = fullsum / 10;

    if (fullsum - sum * 10 > 4)
        sum++;
    return sum;
}

double PPGRealProcess1::findMax(double* X,int X_Len)
{
    double res = X[0];
    int i;
    for (i = 1; i < X_Len; i++)
    {
        if (res < X[i])
            res = X[i];
    }
    return res;
}
double PPGRealProcess1::findMin(double* X,int X_Len)
{
    // Finds the minimum around the center of the buffer
    double res = X[0];
    unsigned char i;
    for (i = 1; i < X_Len; i++)
    {
        if (res > X[i])
            res = X[i];
    }
    return res;
}

QList<int> PPGRealProcess1::getPeakPos()
{
    return peakPos;
}
